package de.fhin.mas.agents.navigator;

import jade.lang.acl.ACLMessage;
import de.fhin.mas.MASEvents;
import de.fhin.mas.MASLocations;
import de.fhin.mas.MASServices;
import de.fhin.mas.agents.MASBaseAgent;
import de.fhin.mas.agents.navigator.behaviours.AbortAndGotoHandler;
import de.fhin.mas.agents.navigator.behaviours.CFPMovementNecessary;
import de.fhin.mas.agents.navigator.behaviours.ChangeOfLocNecessaryHandler;
import de.fhin.mas.agents.navigator.behaviours.Drive;
import de.fhin.mas.agents.navigator.behaviours.MsgHandlerCyclic;
import de.fhin.mas.agents.navigator.behaviours.New_OrderCFPHandler;
import de.fhin.mas.agents.navigator.behaviours.ObjectOutOfReachHandler;
import de.fhin.mas.agents.navigator.behaviours.ResumeCFPHandler;
import de.fhin.mas.agents.navigator.behaviours.SystemStopRequiredHandler;
import de.fhin.mas.agents.navigator.behaviours.UpdatePosition;
import de.fhin.mas.util.Coordinate;
import de.fhin.mas.util.RelativeCoordinate;

/**
 * @author Mike
 * @author Matthias Wilhelm
 * 
 */
public class NavigatorAgent extends MASBaseAgent {

	public NavigatorAgent() {
		super(MASServices.NAVIGATOR);
		myPosition = new Coordinate(0, 0);
		myDirection = 90;
		myDestintation = new Coordinate(10, 10);
		nextMove = myPosition.get_relativeCoordinate(myDestintation);
		drive_to = MASLocations.LOCATION_NONE.getType();
		hardwareDamaged = false;
		cfpStarted = false;
		driving = false;
		driveBehaviour = new Drive(this);
		movementNecessary = new CFPMovementNecessary(this);
		updatePosition = new UpdatePosition(this, 600);
	}

	/**
	 * 
	 */
	private static final long serialVersionUID = 2449672112186855905L;

	/**
	 * Typ des Ziels zu dem gefahren wird
	 */
	private int drive_to = MASLocations.LOCATION_NONE.getType();

	/** Wo sich der Roboter gerade befindet */
	private Coordinate myPosition;
	/** Wo der Roboter hinfahren soll, z.B. zum Objekt */
	private Coordinate myDestintation;
	/** Der Lagerplatz, der nach Greifen des Objekts angefahren werden soll */
	private Coordinate storageLocation;
	/**
	 * Richtung in die der Roboter zeigt <br />
	 * 0° Nord <br />
	 * 90° Ost<br />
	 * 180° Süd<br/>
	 * 270° West
	 */
	private int myDirection;

	/**
	 * Der nächste Punkt, den der Roboter versucht anzufahren.
	 */
	private RelativeCoordinate nextMove;
	/** Data from the area scan **/
	private RelativeCoordinate[] areaScan;
	/**
	 * Flag ob der Antrieb des Roboters beschädigt wurde. Kann auch verwendet
	 * werden, wenn die Reifen durchdrehen.
	 */
	private boolean hardwareDamaged;
	/** Flag, ob cfp zum losfahren schon geschickt wurde */
	private boolean cfpStarted;
	/** Flag, ob sich die Plattform bewegt */
	private boolean driving;

	private final Drive driveBehaviour;
	private final CFPMovementNecessary movementNecessary;
	private final UpdatePosition updatePosition;

	@Override
	protected void addBehaviours() {

		addBehaviour(new MsgHandlerCyclic());
		addBehaviour(new ResumeCFPHandler(this));
		addBehaviour(new New_OrderCFPHandler(this));
		addBehaviour(new AbortAndGotoHandler(this));
		addBehaviour(new ObjectOutOfReachHandler(this));
		addBehaviour(new ChangeOfLocNecessaryHandler(this));
		addBehaviour(new SystemStopRequiredHandler(this));

	}

	@Override
	protected void addTopicSubsciptions() {
		subscribeToTopic(MASEvents.NEW_ORDER);
		subscribeToTopic(MASEvents.RESUME);
		subscribeToTopic(MASEvents.ABORT_AND_GO_TO);
		subscribeToTopic(MASEvents.CHANGE_OF_LOCATION_NECESSARY);
		subscribeToTopic(MASEvents.TARGET_FOUND);
		subscribeToTopic(MASEvents.ENVIRONMENT_SCAN_FINISHED);
		subscribeToTopic(MASEvents.TARGET_OBJECT_REACHED);
		subscribeToTopic(MASEvents.STORAGE_REACHED);
		subscribeToTopic(MASEvents.CHARGING_STATION_REACHED);
		subscribeToTopic(MASEvents.OBJECT_OUT_OF_REACH);
		subscribeToTopic(MASEvents.OBJECT_LOST);

	}

	/**
	 * Setzt Ziel
	 * 
	 * @param drive_to
	 *            eine der vorgegebenen Zielnamen
	 */
	public void setTarget(int drive_to) {
		this.drive_to = drive_to;
	}

	public void set_myPosition(Coordinate newPosition) {
		myDirection = (int) Math.round(this.myPosition.angle(newPosition));
		this.myPosition = newPosition;
	}

	public void set_myDestination(Coordinate myDestination) {
		this.myDestintation = myDestination;
	}

	public void set_myDestination(RelativeCoordinate relativeDestination) {
		myDestintation = relativeDestination.getCoordinate(myDirection);
		myDestintation.add(myPosition);
	}

	private void set_nextMove(RelativeCoordinate nextMove) {
		this.nextMove = nextMove;
	}

	private void set_nextMove(int degree, int distance) {
		nextMove.set_RelativeCoordinate(degree, distance);
	}

	public void set_myDirection(int direction) {
		this.myDirection = direction % 360;
	}

	public void set_hardwareDamaged(boolean hardwareDamaged) {
		this.hardwareDamaged = hardwareDamaged;
	}

	public void set_cfpStarted(boolean cfpStarted) {
		this.cfpStarted = cfpStarted;
	}

	public void set_storageLocation(Coordinate storageLocation) {
		this.storageLocation = storageLocation;
		println("Storage location was set to "
				+ this.storageLocation.toString());
	}

	public void setAreaScan(RelativeCoordinate[] areaScan) {
		this.areaScan = new RelativeCoordinate[areaScan.length];
		this.areaScan = areaScan;
	}

	public boolean get_hardwareDamaged() {
		return hardwareDamaged;
	}

	public int get_myDirection() {
		return myDirection;
	}

	public Coordinate get_myPosition() {
		return myPosition;
	}

	public Coordinate get_myDestination() {
		return myDestintation;
	}

	public boolean get_cfpStarted() {
		return cfpStarted;
	}

	public Coordinate get_storageLocation() {
		return storageLocation;
	}

	public RelativeCoordinate get_nextMove() {
		return nextMove;
	}

	/**
	 * Startet das CFP um zu fragen ob losgefahren werden darf.
	 */
	private void movementNecessary() {
		if (!cfpStarted) {
			cfpStarted = true;
			movementNecessary.reset();
			addBehaviour(movementNecessary);
		}
	}

	/**
	 * Wird aufgerufen, wenn ein Refuse empfangen wird. Resetet den CFP und
	 * startet ihn nach einer Sekunde neu
	 */
	public void cfpMovementRefuse() {
		removeMovementNecessaryCFP();
		try {
			Thread.sleep(1000);
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		movementNecessary();
	}

	/** Resetet die CFP-Behaviour und macht es möglich diese neu zu starten */
	private void removeMovementNecessaryCFP() {
		cfpStarted = false;
		removeBehaviour(movementNecessary);
		movementNecessary.reset();

	}

	/** Entfernt die beiden zur fahrt benötigten Behaviours */
	private void removeDrive() {
		driving = false;
		this.removeBehaviour(driveBehaviour);
		this.removeBehaviour(updatePosition);
	}

	/** wird aufgerufen, wenn der CFP bestätigt wurde. */
	public void readyToDrive() {
		if (!driving) {
			driving = true;
			println("Driving " + driveTo() + "@ " + myDestintation.toString()
					+ ".");

			addBehaviour(driveBehaviour);
			addBehaviour(updatePosition);
		}
	}

	/***
	 * Schickt eine Inform-Message raus, in der steht welches Ziel erreicht
	 * wurde
	 * 
	 */
	public void target_reached() {

		ACLMessage msg = new ACLMessage(ACLMessage.INFORM);

		if (drive_to == MASLocations.LOCATION_RANDOM.getType()) {
			randomDrive();

		} else {
			if (drive_to == MASLocations.LOCATION_OBJECT.getType()) {
				println("Target object reached @ " + myPosition.toString()
						+ ".");
				msg
						.setConversationId(MASEvents.TARGET_OBJECT_REACHED
								.getName());
				sendToAll(msg);
			} else if (drive_to == MASLocations.LOCATION_CHARGER.getType()) {
				msg.setConversationId(MASEvents.CHARGING_STATION_REACHED
						.getName());
				sendToAll(msg);

			} else if (drive_to == MASLocations.LOCATION_STORAGE.getType()) {
				msg.setConversationId(MASEvents.STORAGE_REACHED.getName());
				sendToAll(msg);

			} else if (drive_to == MASLocations.LOCATION_USERDEFINED.getType()) {

			} else if (drive_to == MASLocations.LOCATION_NONE.getType()) {

			}
			stop();
		}

	}

	/**
	 * Stoppt den Roboter.
	 */
	public void stop() {
		println("Platform stopped.");
		drive_to = MASLocations.LOCATION_NONE.getType();

		removeMovementNecessaryCFP();
		removeDrive();
	}

	/**
	 * Wird bei einem system stop aufgerufen. Pause, weil nicht gewiss ist ob
	 * die fahrt fortgesetzt werden soll
	 */
	public void pause() {
		println("Platform paused.");
		removeMovementNecessaryCFP();
		removeDrive();

	}

	/**
	 * Wird aufgerufen, wenn die Fahrt nach einem system stop fortgesetzt werden
	 * soll
	 */
	public void resume() {
		println("Resume driving " + driveTo() + " @"
				+ myDestintation.toString());
		movementNecessary();
	}

	/**
	 * Faehrt zufaellige Koordinate an
	 */
	public void randomDrive() {
		myDestintation.set_Coordinate(
				(int) Math.round(Math.random() * 10000) % 1000, (int) Math
						.round(Math.random() * 10000) % 1000);
		drive_to = MASLocations.LOCATION_RANDOM.getType();
		movementNecessary();

	}

	/** Rechnet relative Koordinate aus, zu welcher der Roboter fahren soll */
	public void nextMove() {
		nextMove = myPosition.get_relativeCoordinate(myDestintation);
		nextMove.addAngle(myDirection);
	}

	/**
	 * Setzt die Koordinaten für das Lager und startet die Fahrt dorthin.
	 */
	public void target_storage() {
		myDestintation = storageLocation;
		nextMove();
		println("Driving to storage at " + storageLocation.toString());
		movementNecessary();
		drive_to = MASLocations.LOCATION_STORAGE.getType();
	}

	public void target_object() {
		nextMove();
		movementNecessary();
		drive_to = MASLocations.LOCATION_OBJECT.getType();

	}

	public void target_charger() {
		nextMove();
		movementNecessary();
		drive_to = MASLocations.LOCATION_CHARGER.getType();
	}

	public void target_none() {
		nextMove();
		movementNecessary();
		drive_to = MASLocations.LOCATION_NONE.getType();
	}

	/**
	 * Versucht 5cm nach vorne zu fahren und startet die Fahrt.
	 */
	public void getCloser() {
		// 5 cm näher Fahren
		set_nextMove(0, 5);
		movementNecessary();
		drive_to = MASLocations.LOCATION_OBJECT.getType();
	}

	/** Gibt als String zurück welches Ziel gerade angefahren wird */
	private String driveTo() {
		String target = null;
		if (drive_to == MASLocations.LOCATION_NONE.getType()) {
			target = "";
		} else if (drive_to == MASLocations.LOCATION_OBJECT.getType()) {
			target = "to object";
		} else if (drive_to == MASLocations.LOCATION_CHARGER.getType()) {
			target = "to charging station";
		} else if (drive_to == MASLocations.LOCATION_STORAGE.getType()) {
			target = "to storage location";
		} else if (drive_to == MASLocations.LOCATION_USERDEFINED.getType()) {
			target = "to userdefined location";
		} else if (drive_to == MASLocations.LOCATION_RANDOM.getType()) {
			target = "to random position";
		} else {
			target = "";
		}
		return target;
	}

	private void checkAreaScan() {
		RelativeCoordinate frontLeft[] = new RelativeCoordinate[areaScan.length / 4];
		RelativeCoordinate frontRight[] = new RelativeCoordinate[areaScan.length / 4];
		RelativeCoordinate sideLeft[] = new RelativeCoordinate[areaScan.length / 6];
		RelativeCoordinate sideRight[] = new RelativeCoordinate[areaScan.length / 6];
		RelativeCoordinate rearRight[] = new RelativeCoordinate[areaScan.length / 6];
		RelativeCoordinate rearLeft[] = new RelativeCoordinate[areaScan.length / 6];
		int fLeft = 0, fRight = 0, sLeft = 0, sRight = 0, rLeft = 0, rRight = 0;
		for (RelativeCoordinate obstacle : areaScan) {
			// Nach links-, rechts-Vorne und links-, rechts-Hinten trennen
			if (obstacle.getAngle() >= -45 && obstacle.getAngle() <= 0) {
				if (obstacle.getDistance() > 0) {
					frontLeft[fLeft++] = obstacle;
				}
			} else if (obstacle.getAngle() > 0 && obstacle.getAngle() <= 45) {
				if (obstacle.getDistance() > 0) {
					frontRight[fRight++] = obstacle;
				}
			} else if (obstacle.getAngle() > 45 && obstacle.getAngle() <= 135) {
				if (obstacle.getDistance() > 0) {
					sideLeft[sLeft++] = obstacle;
				}
			} else if (obstacle.getAngle() < -45 && obstacle.getAngle() >= -135) {
				if (obstacle.getDistance() > 0) {
					sideRight[sRight++] = obstacle;
				}
			} else if (obstacle.getAngle() < -135) {
				if (obstacle.getDistance() > 0) {
					rearLeft[rLeft++] = obstacle;
				}
			} else if (obstacle.getAngle() > 135) {
				if (obstacle.getDistance() > 0) {
					rearRight[rRight++] = obstacle;
				}
			}

			nextMove.setAngle(safeAngle(frontRight));
			nextMove.setAngle(safeAngle(frontLeft));
			nextMove.setAngle(safeAngle(sideRight));
			nextMove.setAngle(safeAngle(sideLeft));
		}

	}

	private int safeAngle(RelativeCoordinate[] obstacles) {
		for (RelativeCoordinate obstacle : obstacles) {
			if (obstacle.getCoordinate(myDirection).getX() < 100) {
				if (obstacle.getDistance() != 0) {
					return obstacle.getAngle() / obstacle.getDistance();
				} else {
					return -obstacle.getAngle();
				}
			}
		}
		return 0;
	}

	public void println(String message) {
		System.out.println("-" + getAID().getName() + ":\t" + message);
	}

}